import remote_con.ybc as ybc
import threading
import time
import socket
import struct

car = ybc.YB_Pcb_Car()
forward    = False
backward   = False
right      = False
left       = False
do_inplace_right = False
do_inplace_left  = False


base_speed = 40
diff_speed = 60
fowrd_time = .2

def inplace_left(car):
    car.Car_Run(base_speed, base_speed + diff_speed)
    time.sleep(fowrd_time)
    car.Car_Back(base_speed  + diff_speed, base_speed)
    time.sleep(fowrd_time+.05)
    car.Car_Stop()

def inplace_right(car):
    car.Car_Run(base_speed + diff_speed, base_speed )
    time.sleep(fowrd_time)
    car.Car_Back(base_speed, base_speed + diff_speed)
    time.sleep(fowrd_time+.05)
    car.Car_Stop()


class RemoteInputThread(threading.Thread):
    
    def __init__(self):
        super(RemoteInputThread, self).__init__()
        
        self._quit = False

    def set_quit(self):
        self._quit = True

    def run(self):
        global forward
        global backward
        global right
        global left
        global do_inplace_right
        global do_inplace_left
        self.s = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        self.s.bind(('',2334))  #现在改成下位机监听某一端口，这个端口应该告知SP
        print('Service Started')
        print('listening on 2334')
        self.s.listen(1)
        while not self._quit:
            server, addr = self.s.accept()
            print(f'Accepted {addr}')
            while not self._quit:
                raw_data = b''
                try:
                    raw_data = server.recv(4)
                except:
                    server.close()
                    print('Unexcepet error')
                    forward = False
                    backward = False
                    left = False
                    right = False
                    break
                status_word = 0
                if len(raw_data) == 0:
                    print('EOF')
                try:
                    status_word = struct.unpack('<i',raw_data)[0]
                except:
                    print('misformat pack')
                    forward = False
                    backward = False
                    left = False
                    right = False
                    break
                #continue
                if status_word & 0b000001 != 0:
                    forward = True
                    print('set forward')
                else:
                    forward = False
                    print('unset forward')

                if status_word & 0b000010 != 0:
                    backward = True
                else:
                    backward = False
                if status_word & 0b000100 != 0:
                    right = True
                    left = False
                elif status_word & 0b001000 != 0:
                    left = True
                    right = False
                else:
                    left = False
                    right = False
                if status_word & 0b010000 != 0:#In place right
                    inplace_right(car)
                    server.sendall(struct.pack('<i', 1))
                    print('debug right')
                if status_word & 0b100000 != 0:#In place left
                    inplace_left(car)
                    server.sendall(struct.pack('<i', 2))
                    print('debug left')
            print('closed connection')       
            server.close()
_idle = True

js = RemoteInputThread()
js.start()
speed_factor = 40
try:
    while True:
        if do_inplace_right:
            print('[info] execute inplace right')
            inplace_right(car)
            do_inplace_right = False
        elif do_inplace_left:
            print('[info] execute inplace left')
            inplace_left(car)
            do_inplace_left = False
        elif not forward and not backward:
            #print('idle')
            if not _idle:
                car.Car_Stop()
                _idle = True
            pass
        elif forward and not backward:
            _idle = False
            if right:
                #print('right')
                car.Car_Run(speed_factor + diff_speed, speed_factor)
            elif left:
                #print('left')
                car.Car_Run(speed_factor, speed_factor + diff_speed)            
            else:
                print('forward')
                car.Car_Run(speed_factor, speed_factor)
        elif backward and not forward:
            _idle = False
            if right:
                #print('backward right')
                car.Car_Back(speed_factor  + diff_speed, speed_factor)
            elif left:
                #print('backward left')
                car.Car_Back(speed_factor, speed_factor  + diff_speed)
            else:
                #print('backward')
                car.Car_Back(speed_factor, speed_factor)    
        elif backward and forward:
            _idle = False
            car.Car_Stop()
        else:
            car.Car_Stop()
        time.sleep(.01)
except KeyboardInterrupt as _:
    js.set_quit()